/***************************************************************************\
 * Copyright (C) by Keio University
 * CalibrationMatrix.hpp created in 12 2011.
 * Mail : fdesorbi@hvrl.ics.keio.ac.jp
 *
 * CalibrationMatrix.hpp is part of the HVRL Engine Project.
 *
 * The HVRL Engine Project is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * The HVRL Engine Project is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
\***************************************************************************/

#include "hvrl/computer-vision/CalibrationMatrix.hpp"

namespace hvrl
{

CalibrationMatrix::CalibrationMatrix(void)
{
    this->T = cv::Mat::eye(cv::Size(3,4),cv::DataType<float>::type);
    this->K = cv::Mat::eye(cv::Size(3,3),cv::DataType<float>::type);
    this->P = cv::Mat::eye(cv::Size(3,4),cv::DataType<float>::type);
    inverseKT();
    extractRtC();

}

CalibrationMatrix::CalibrationMatrix(const cv::Mat& P) throw(Exception)
{
    //Check the size of P, should be 3x4
    if(P.rows!=3 || P.cols!=4)
    {
        throw(Exception("CalibrationMatrix::CalibrationMatrix","Size of the projection matrix is incorrect"));
    }

    //Decompose P, insert directly result in K
    cv::Mat R,C;
    cv::decomposeProjectionMatrix( P, this->K, R, C);

    // Unhomogeneous coordinates
    float d = 1.0f / C.at<float>(3,0);
    cv::Mat t(3,1,cv::DataType<float>::type);
    t.at<float>(0,0) = C.at<float>(0,0)*d;
    t.at<float>(1,0) = C.at<float>(1,0)*d;
    t.at<float>(2,0) = C.at<float>(2,0)*d;

    // Transform center of camera to translation vector
    t = -R*t;

    // Copy the result [R|t] in T
    this->T = cv::Mat(3,4,cv::DataType<float>::type);
    for(unsigned int i = 0; i < 3; ++i)
    {
        for(unsigned int j = 0; j < 3; ++j)
        {
            this->T.at<float>(i,j) = R.at<float>(i,j);
        }
        this->T.at<float>(i,3) = t.at<float>(i,0);
    }

    inverseKT();
    extractRtC();

    this->P = P;
}

CalibrationMatrix::CalibrationMatrix(const cv::Mat& K, cv::Mat& T) throw(Exception)
{
    //Check the size of K, should be 3x3
    if(K.rows!=3 || K.cols!=3)
    {
        throw(Exception("CalibrationMatrix::CalibrationMatrix","Size of the intrinsic parameters matrix is incorrect"));
    }

    //Check the size of T, should be 3x4
    if(T.rows!=3 || T.cols!=3)
    {
        throw(Exception("CalibrationMatrix::CalibrationMatrix","Size of the extrinsic parameters matrix is incorrect"));
    }

    this->K = K;
    this->T = T;
    this->P = K*T;

    inverseKT();
    extractRtC();

}

CalibrationMatrix::~CalibrationMatrix(void)
{
}

void CalibrationMatrix::extractRtC(void)
{
    this->R = this->T.colRange(0,2);
    this->t = this->T.col(3);
    this->R_inverse = this->R.inv(cv::DECOMP_SVD);
    this->C = -this->R_inverse*this->t;
}


void CalibrationMatrix::inverseKT(void)
{
    this->K_inverse = this->K.inv(cv::DECOMP_SVD);
    this->T_inverse = this->T.inv(cv::DECOMP_SVD);
}



}
